/*
 * Copyright (c) 2020 - ~, HIT_HERO Team
 *
 * PID MATH MODULE SOUCE FILE
 * Used in RT-Thread Operate System
 *
 * Change Logs:
 * Date           Author       Notes            	Mail
 * 2020-08-05     WangXi       first version    	WangXi_chn@foxmail.com
 */
#include "Math_PID.h"
#include <math.h>

static void Module_PIDInit(MATH_PID *module);
static void Module_PIDUpdate(MATH_PID *module,float Aim, float Feedback);

/* Global Method */
rt_err_t Module_PID_Config(MATH_PID *module)
{
	if( module->Method_Init		==NULL &&
		module->Method_Update		==NULL 
		
    ){  
        /* Link the Method */
        module->Method_Init 		= Module_PIDInit;
		module->Method_Update 	= Module_PIDUpdate;
		
    }
    else{
        rt_kprintf("Warning: Module Motor 3508 is Configed twice\n");
        return RT_ERROR;
    }

    /* Device Init */
    module->Method_Init(module);

    return RT_EOK;
}

static void Module_PIDInit(MATH_PID *module)
{
	module->Value_Actual = 0;
	module->Value_Aim = 0;
	module->Value_err = 0;
	module->Value_err_last = 0;
	module->Value_integral = 0;
	module->Value_output = 0;
}

static void Module_PIDUpdate(MATH_PID *module,float Aim, float Feedback)
{
	float P,I,D;
	
	module->Value_Aim = Aim;
	module->Value_Actual = Feedback;
	
	/* Culculate P */	
	if(module->Value_Aim > module->Property_AimMax)
		module->Value_Aim = module->Property_AimMax;
	else if(module->Value_Aim < -module->Property_AimMax)
		module->Value_Aim = -module->Property_AimMax;
	
	module->Value_err = module->Value_Aim - module->Value_Actual;
	
	P = module->Value_err * module->Property_Kp;
	
	/* Culculate I */
	if(fabs(module->Value_err) < module->Property_integralErrMax)
	{
		module->Value_integral += module->Value_err * module->Property_Ki * module->Property_dt;
	
		if(module->Value_integral > module->Property_integralMax)
			module->Value_integral = module->Property_integralMax;
		else if(module->Value_integral < -module->Property_integralMax)
			module->Value_integral = -module->Property_integralMax;
	
		I = module->Value_integral;
	}
	
	/* Culculate D */
	D = module->Property_Kd * (module->Value_err - module->Value_err_last) / module->Property_dt;
	
	/* Culculate Output */
	module->Value_output = P + I + D;
		
	if(module->Value_output > module->Property_OutputMax)
		module->Value_output = module->Property_OutputMax;
	else if(module->Value_output < -module->Property_OutputMax)
		module->Value_output = -module->Property_OutputMax;
}

/************************ (C) COPYRIGHT 2020 WANGXI **************END OF FILE****/

